#include "utils.h"
#include "base/common.h"
#include "base/geometry.h"
#include "base/defines.h"
#include "base/macro.h"
#include "base/log.h"
#include "ipm.h"

#include <map>
#include <fstream>
#include <functional>
#include <opencv2/core/core_c.h>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#include <autosdk_hdmap/hdmap_layer_api.h>

using namespace LFM;

std::vector<hdmap_lane> utils::MergeHDlinks(std::vector<hdmap_lane> &lanes, double tolerance)
{
	if(lanes.empty())
		return std::vector<hdmap_lane>();

	for (std::size_t i = 0; i < lanes.size() - 1; i++)
	{
		if (lanes[i].geometry.empty())
			continue;

		for (std::size_t j = 0; j < lanes.size(); j++)
		{
			if (i == j || lanes[j].geometry.empty())
				continue;

			Eigen::Vector3d S_point_i = lanes[i].geometry[0];
			Eigen::Vector3d E_point_i = lanes[i].geometry[lanes[i].geometry.size() - 1];
			Eigen::Vector3d S_point_j = lanes[j].geometry[0];
			Eigen::Vector3d E_point_j = lanes[j].geometry[lanes[j].geometry.size() - 1];

			if ((S_point_i - E_point_j).norm() < 1e-1)
			{
				lanes[i].mark_type == lanes[i].mark_type | lanes[j].mark_type;
				lanes[j].geometry.insert(lanes[j].geometry.end(), lanes[i].geometry.begin(), lanes[i].geometry.end());
				lanes[i].geometry.swap(lanes[j].geometry);
				lanes[j].geometry.clear();
			}
			else if ((E_point_i - S_point_j).norm() < 1e-1)
			{
				lanes[i].mark_type == lanes[i].mark_type | lanes[j].mark_type;
				lanes[i].geometry.insert(lanes[i].geometry.end(), lanes[j].geometry.begin(), lanes[j].geometry.end());
				lanes[j].geometry.clear();
			}
			else if ((S_point_i - S_point_j).norm() < 1e-1 && (E_point_i - E_point_j).norm() < 1e-1)
			{
				lanes[j].geometry.clear();
			}
		}
	}

	std::vector<hdmap_lane> res;
	for (std::size_t i = 0; i < lanes.size(); i++)
	{
		if (!lanes[i].geometry.empty())
		{
			res.push_back(std::move(lanes[i]));
		}
	}
	return res;
}

std::vector<ARSLane> utils::MergeDLlinks(std::vector<ARSLane> &lanes, double tolerance)
{
	for (std::size_t i = 0; i < lanes.size(); i++)
	{
		if (lanes[i].geometry3D.empty())
			continue;

		for (std::size_t j = 0; j < lanes.size(); j++)
		{
			if (i == j || lanes[j].geometry3D.empty())
				continue;

			Eigen::Vector3d S_point_i = lanes[i].geometry3D[0];
			Eigen::Vector3d E_point_i = lanes[i].geometry3D[lanes[i].geometry3D.size() - 1];
			Eigen::Vector3d S_point_j = lanes[j].geometry3D[0];
			Eigen::Vector3d E_point_j = lanes[j].geometry3D[lanes[j].geometry3D.size() - 1];

			if (geometry::DistancePtToLine3D(S_point_i, S_point_j, E_point_j) < tolerance && geometry::DistancePtToLine3D(E_point_i, S_point_j, E_point_j) < tolerance)
			{
				double dis_s_e = (S_point_i - E_point_j).norm();
				double dis_e_s = (E_point_i - S_point_j).norm();
				if (dis_s_e < dis_e_s)
				{
					lanes[i].mark_type == lanes[i].mark_type | lanes[j].mark_type;
					lanes[j].geometry3D.insert(lanes[j].geometry3D.end(), lanes[i].geometry3D.begin(), lanes[i].geometry3D.end());
					lanes[i].geometry3D.swap(lanes[j].geometry3D);
					lanes[j].geometry3D.clear();
				}
				else
				{
					lanes[i].mark_type == lanes[i].mark_type | lanes[j].mark_type;
					lanes[i].geometry3D.insert(lanes[i].geometry3D.end(), lanes[j].geometry3D.begin(), lanes[j].geometry3D.end());
					lanes[j].geometry3D.clear();
				}
			}
		}
	}

	std::vector<ARSLane> res;
	for (std::size_t i = 0; i < lanes.size(); i++)
	{
		if (!lanes[i].geometry3D.empty() && geometry::GetLength(lanes[i].geometry3D) > 10.0)
		{
			res.push_back(lanes[i]);
		}
	}
	return res;
}

Eigen::Vector3d utils::WorldToCamera(const Eigen::Vector3d &xw3,
									 Eigen::Matrix3d wr,
									 Eigen::Vector3d wt)
{
	Eigen::Vector3d cx = wr.transpose() * (xw3 - wt);
	return cx;
}

Eigen::Vector3d utils::CameraToWorld(const Eigen::Vector3d &xc3, Eigen::Matrix3d wr, Eigen::Vector3d wt)
{
	Eigen::Vector3d wx = wr * xc3 + wt;
	return wx;
}

void LFM::utils::saveAsOBJ(const std::string &filename, const std::vector<std::vector<Eigen::Vector3d>> &polylines)
{
	if (polylines.size() == 0)
	{
		std::cout << "no 3D lines to save!" << std::endl;
		return;
	}

	std::ofstream file;
	file.open(filename);
	file.precision(20);

	size_t lineID = 0;
	size_t pointID = 1;
	std::map<size_t, size_t> lines2points;

	for (std::size_t i = 0; i < polylines.size(); i++)
	{
		if (polylines[i].size() >= 2)
		{
			for (std::size_t j = 0; j < polylines[i].size() - 1; j++, lineID++, pointID += 2)
			{
				Eigen::Vector3d P1 = polylines[i][j];
				Eigen::Vector3d P2 = polylines[i][j + 1];

				file << "v " << P1.x() << " " << P1.y() << " " << P1.z() << std::endl;
				file << "v " << P2.x() << " " << P2.y() << " " << P2.z() << std::endl;

				lines2points[lineID] = pointID;
			}
		}
	}

	std::map<size_t, size_t>::const_iterator it = lines2points.begin();
	for (; it != lines2points.end(); ++it)
	{
		file << "l " << it->second << " " << it->second + 1 << std::endl;
	}

	file.close();
}

std::vector<ARSLane> utils::FittingSpec(const DlLane &dlLane, cv::Mat K, cv::Mat D)
{
	if (dlLane.polygon.size() < 2)
	{
		LOG("the polygon size is small than two, fitting failed!") << std::endl;
		return std::vector<ARSLane>();
	}

	/*std::vector<cv::Point2d> undistort_points, cv_polygon;
	std::vector<Eigen::Vector2d> polygon = dlLane.polygon;
	for(Eigen::Vector2d v : polygon)
	{
		cv_polygon.emplace_back(v.x(), v.y());
	}
	cv::undistortPoints(cv_polygon, undistort_points, K, D, cv::noArray(), K);*/

	std::vector<Eigen::Vector2d> undistort_points = dlLane.calibrated_polygon;
	int n = undistort_points.size();
	Eigen::Vector2d mean(0, 0);
	for (int i = 0; i < n; i++)
	{
		mean += Eigen::Vector2d(undistort_points[i].x(), undistort_points[i].y());
	}
	mean = mean / n;

	Eigen::MatrixXd Xs = Eigen::MatrixXd::Zero(2, n);
	for (int i = 0; i < n; i++)
	{
		Xs(0, i) = undistort_points[i].x() - mean.x();
		Xs(1, i) = undistort_points[i].y() - mean.y();
	}

	Eigen::MatrixXd C = Xs * Xs.transpose() * 1 / n;
	Eigen::EigenSolver<Eigen::MatrixXd> es(C);
	Eigen::EigenSolver<Eigen::MatrixXd>::EigenvalueType evs = es.eigenvalues();

	int maxPos = 0, minPos = 0;
	evs.real().maxCoeff(&maxPos);
	evs.real().minCoeff(&minPos);

	Eigen::Vector2d Axis_x = es.eigenvectors().col(maxPos).real();
	Axis_x.normalize();

	Eigen::Vector2d Axis_y = es.eigenvectors().col(minPos).real();
	Axis_y.normalize();

	std::vector<ARSLane> cluster_splines;
	double mse = std::sqrt(evs[minPos].real());
	if (mse < 20.0)
	{
		ARSLane lane = Fitting2d(undistort_points, K, D);
		if(!lane.geometry2D.empty())
		{
			lane.mark_type = dlLane.mark_type;
			cluster_splines.push_back(lane);
		}
		return cluster_splines;
	}

	std::vector<std::vector<Eigen::Vector2d>> cluster_sets(2);
	for (Eigen::Vector2d pt : undistort_points)
	{
		double y = (pt - mean).dot(Axis_y);
		if (y > 0)
		{
			cluster_sets[0].emplace_back(pt);
		}
		else if (y < 0)
		{
			cluster_sets[1].emplace_back(pt);
		}
	}

	for (std::size_t i = 0; i < cluster_sets.size(); i++)
	{
		ARSLane lane = Fitting2d(cluster_sets[i], K, D);
		if(!lane.geometry2D.empty())
		{
			lane.mark_type = dlLane.mark_type;
			cluster_splines.push_back(lane);
		}
	}
	return cluster_splines;
}

ARSLane utils::Fitting2d(const std::vector<Eigen::Vector2d>& geometry2d, cv::Mat K, cv::Mat D)
{
	std::vector<Eigen::Vector2d> fitting_line;
	int maxRow = 0, minRow = 10000, maxCol = 0, minCol = 10000;
	std::vector<cv::Point2d> points2d;
	for (Eigen::Vector2d pix2d : geometry2d)
	{
		if (pix2d.x() > maxCol)
			maxCol = pix2d.x();
		if (pix2d.x() < minCol)
			minCol = pix2d.x();
		if (pix2d.y() > maxRow)
			maxRow = pix2d.y();
		if (pix2d.y() < minRow)
			minRow = pix2d.y();
		points2d.emplace_back(pix2d.x(), pix2d.y());
	}

	cv::Vec4f args;
	cv::fitLine(points2d,
				args,
				CV_DIST_L2,
				0.0, 0.01, 0.01);
	
	int sample_cnt = 20;
	double sample_interval = (maxRow - minRow) / sample_cnt;
	double b = -args[0], a = args[1];
	double c = -a * args[2] - b * args[3];
	
	ARSLane lane;
	lane.line_arg2D = Eigen::Vector3d(a, b, c);

	for (std::size_t i = 0; i <= sample_cnt; i++)
	{
		double y = maxRow - i * sample_interval;
		if(std::fabs(a) > EPS)
		{
			double x = (-c - b * y) / a;
			lane.geometry2D.emplace_back(x, y);
		}
	}
	return lane;
}

bool utils::Recontruction(std::vector<ARSLane>& lane3Ds, const Camera &cam, const Mounting& mount,double scope)
{
	IPM ipm;
	ipm.CalMatrix(cam);

	cv::Mat K = cam.get_K(), D = cam.get_D();
	auto imgToBody = [&](Eigen::Vector2d xi, Eigen::Vector3d &c3d, double sight) -> bool 
	{
		/*cv::Mat xi3 = (cv::Mat_<double>(3, 1) << xi.x(), xi.y(), 1.0);
		cv::Mat xc3 = K.inv() * xi3;

		cv::Vec2d p;
		p[0] = xc3.at<double>(0, 0);
		p[1] = xc3.at<double>(1, 0);

		Eigen::Vector3d xc(p[0], p[1], 1.0);
		Eigen::Vector3d trans = mount.translation;
		Eigen::Vector3d rot_xb = mount.rotation.matrix() * xc;
		if (std::fabs(rot_xb.z()) > EPS)
		{
			double scale = -trans.z() / rot_xb.z();
			rot_xb = scale * rot_xb + trans;
			double r = rot_xb.x();

			if (r <= sight && r > 0)
			{
				c3d.x() = rot_xb.x();
				c3d.y() = rot_xb.y();
				c3d.z() = rot_xb.z();
				return true;
			}
		}
		return false;*/

		c3d = ipm.Mapping(xi);
		if (c3d.x() <= sight && c3d.x() > 0)
		{
			return true;
		}
		return false;
	};

	for (ARSLane &lane : lane3Ds)
	{
		std::vector<Eigen::Vector3d> wSpline;
		for (Eigen::Vector2d p : lane.geometry2D)
		{
			Eigen::Vector3d b3d;
			if (imgToBody(p, b3d, scope))
			{
				wSpline.push_back(b3d);
			}
		}

		if (wSpline.size() > 1)
		{
			geometry::Resample(wSpline, 3.0);
			lane.geometry3D = wSpline;
		}
	}
	lane3Ds = utils::MergeDLlinks(lane3Ds);

	if (!lane3Ds.empty())
	{
		return true;
	}
	return false;
}

void LFM::utils::MappintDistort(const std::string &filename, cv::Mat K, cv::Mat D, cv::Mat &map11, cv::Mat &map12)
{
	if (!map11.empty() && !map12.empty())
	{
		return;
	}

	cv::Mat img = cv::imread(filename);
	if (img.empty())
	{
		return;
	}

	map11.create(img.size(), CV_32FC1);
	map12.create(img.size(), CV_32FC1);

	initUndistortRectifyMap(K, D, cv::Mat(), K,
							img.size(), CV_16SC2, map11, map12);
}

double utils::Score(const ARSLane &l1,
					const ARSLane &l2,
					const hdmap_lane &r1,
					const hdmap_lane &r2)
{
	if (!geometry::IsOverlap(l1.geometry3D, l2.geometry3D))
		return 0.0;

	if (!geometry::IsOverlap(r1.geometry, r2.geometry))
		return 0.0;

	double head_disL, tail_disL, head_disR, tail_disR;
	geometry::GetHeadTailDistance(l1.geometry3D, l2.geometry3D, head_disL, tail_disL);
	geometry::GetHeadTailDistance(r1.geometry, r2.geometry, head_disR, tail_disR);

	double ratioL = std::fabs(head_disR) > EPS ? head_disL / head_disR : head_disL / EPS;
	if (std::fabs(head_disL - head_disR) > 2.0)
		return 0.0;

	double ratioR = std::fabs(tail_disR) > EPS ? tail_disL / tail_disR : tail_disL / EPS;
	if (std::fabs(tail_disL - tail_disR) > 2.0)
		return 0.0;

	double ratio = (ratioL + ratioR) / 2.0;
	double geometry_score = std::exp(-std::fabs(ratio - 1.0));

	double disl1r1 = geometry::DistancePolylineToPolyline2D(l1.geometry3D, r1.geometry);
	double disl2r2 = geometry::DistancePolylineToPolyline2D(l2.geometry3D, r2.geometry);
	double dislr = (disl1r1 + disl2r2) / 2.0;
	double distance_score = 1.0 / (1.0 + dislr);

	double property_score = 0.0;
	if ((l1.mark_type & r1.mark_type) != 0x0)
		property_score += 0.5;
	if ((l2.mark_type & r2.mark_type) != 0x0)
		property_score += 0.5;

	return geometry_score + distance_score + property_score;
}

bool utils::Matching(const std::vector<ARSLane> &dlLanes, const std::vector<hdmap_lane> &hdLanes, std::vector<match_pair> &match_results)
{
	std::vector<match_pair> candidate_match_pairs;
	for (std::size_t i = 0; i < dlLanes.size(); i++)
	{
		for (std::size_t j = 0; j < hdLanes.size(); j++)
		{
			if (geometry::IsOverlap(dlLanes[i].geometry3D, hdLanes[j].geometry))
			{
				candidate_match_pairs.emplace_back(i, j);
			}
		}
	}

	if (candidate_match_pairs.empty())
	{
		LOG("candidate_match_pairs is empty, match failed!") << std::endl;
		return false;
	}

	unsigned int dim = candidate_match_pairs.size();
	Eigen::MatrixXd affineMtx = Eigen::MatrixXd::Zero(dim, dim);
	for (std::size_t i = 0; i < dim; i++)  
	{
		unsigned int idL1 = candidate_match_pairs[i].source;
		unsigned int idR1 = candidate_match_pairs[i].target;

		for (std::size_t j = i + 1; j < dim; j++)
		{
			unsigned int idL2 = candidate_match_pairs[j].source;
			unsigned int idR2 = candidate_match_pairs[j].target;

			if ((idL1 == idL2) || (idR1 == idR2))
			{
				continue; //not satisfy the one to one match condition
			}

			ARSLane l1 = dlLanes[idL1];
			ARSLane l2 = dlLanes[idL2];
			hdmap_lane r1 = hdLanes[idR1];
			hdmap_lane r2 = hdLanes[idR2];
			affineMtx(i, j) = affineMtx(j, i) = Score(l1, l2, r1, r2);
		}
	}

	std::vector<unsigned int> match_array;
	auto is_exist = [&](unsigned int id) -> bool {
		for (size_t i = 0; i < match_array.size(); i++)
		{
			if (match_array[i] == id)
				return true;
		}
		return false;
	};

	auto is_conflict = [&](unsigned int id) -> bool {
		for (size_t i = 0; i < match_array.size(); i++)
		{
			if (candidate_match_pairs[match_array[i]].source == candidate_match_pairs[id].source || candidate_match_pairs[match_array[i]].target == candidate_match_pairs[id].target)
			{
				return true;
			}
		}
		return false;
	};

	auto is_ambiguous = [&](const std::vector<std::pair<unsigned int, double>> &sort_candidates, double simility_delta) -> bool {
		unsigned int sId = sort_candidates[0].first;
		for (std::size_t i = 1; i < sort_candidates.size(); i++)
		{
			match_pair match_point = candidate_match_pairs[sort_candidates[i].first];
			if (match_point.source == candidate_match_pairs[sId].source || match_point.target == candidate_match_pairs[sId].target)
			{
				double score0 = sort_candidates[0].second;
				double score1 = sort_candidates[i].second;
				double score_delta = score0 - score1;
				if (score_delta < simility_delta)
				{
					return true;
				}
				break;
			}
		}
		return false;
	};
	unsigned int num = 0;
	while (num < dim)
	{
		std::vector<std::pair<unsigned int, double>> sort_candidates;
		for (unsigned int i = 0; i < dim; i++)
		{
			sort_candidates.emplace_back(i, 0.0);
		}
		for (unsigned int i = 0; i < dim; i++)
		{
			if (!is_exist(i) && !is_conflict(i))
			{
				for (int j = 0; j < dim; j++)
				{
					if (!is_conflict(j) || is_exist(j))
					{
						sort_candidates[i].second += affineMtx(i, j);
					}
				}
			}
		}

		std::sort(sort_candidates.begin(), sort_candidates.end(),
				  [=](const std::pair<unsigned int, double> &lhs, const std::pair<unsigned int, double> &rhs) {
					  return lhs.second > rhs.second;
				  });

		if (sort_candidates[0].second < EPS)
		{
			break;
		}

		if (num == 0 && sort_candidates.size() > 1 && hdLanes.size() != dlLanes.size())
		{
			if (is_ambiguous(sort_candidates, 0.7))
			{
				LOG("the matching is ambiguous, matching failed!") << std::endl;
				return false;
			}
		}

		unsigned int sId = sort_candidates[0].first;
		match_array.push_back(sId);
		num++;
	}

	for (unsigned sId : match_array)
	{
		match_results.push_back(candidate_match_pairs[sId]);
	}
	return true;
}

bool utils::FindNearestPoint(const Eigen::Vector3d &ptHit, const hdmap_lane &lane, Eigen::Vector3d &ptPrj, double tolerance)
{
	int nSegIdx = -1;
	Eigen::Vector3d pntPrj;
	if (geometry::GetProjectPointToPolyline(
			ptHit,
			lane.geometry,
			pntPrj,
			nSegIdx))
	{
		double distance2D = std::sqrt((ptHit.x() - pntPrj.x()) * (ptHit.x() - pntPrj.x()) + (ptHit.y() - pntPrj.y()) * (ptHit.y() - pntPrj.y()));
		if (distance2D < tolerance)
		{
			ptPrj = pntPrj;
			return true;
		}
	}
	return false;
}

bool utils::ComputeRT(const std::vector<ARSLane> &polylines, const std::vector<hdmap_lane> &lanes, Eigen::Matrix3d &R, Eigen::Vector3d &T, double &rmse, std::vector<accordance_pair> &match_pair_res, double tolerance)
{
	std::vector<match_pair> match_results;
	if (utils::Matching(polylines, lanes, match_results))
	{
		std::vector<accordance_pair> match_points;
		for (std::vector<match_pair>::iterator it = match_results.begin(); it != match_results.end(); it++)
		{
			unsigned int id1 = it->source, id2 = it->target;
			ARSLane spline = polylines[id1];
			hdmap_lane lane = lanes[id2];
			for (std::size_t i = 0; i < spline.geometry3D.size(); i++)
			{
				Eigen::Vector3d pntPrj;
				Eigen::Vector3d p3d = spline.geometry3D[i];
				if (FindNearestPoint(p3d,
									 lane,
									 pntPrj,
									 tolerance))
				{
					accordance_pair match_point;
					match_point.source = p3d;
					match_point.target = pntPrj;
					match_points.push_back(match_point);
				}
			}
		}

		if (Translate3d_3d_LM(match_points, R, T, rmse))
		{
			match_pair_res = match_points;
			return true;
		}
	}
	return false;
}

bool utils::Translate3d_3d_LM(const std::vector<accordance_pair> &match_points, Eigen::Matrix3d &R, Eigen::Vector3d &T, double& error)
{
	if (match_points.empty())
	{
		LOG("match_points is empty, translate failed!") << std::endl;
		return false;
	}

	int N = match_points.size();
	std::vector<Eigen::Vector3d> q1(N), q2(N);
	for (int i = 0; i < N; i++)
	{
		q1[i] = match_points[i].source;
		q2[i] = match_points[i].target;
	}

	double omga = .0, kappa = .0, t1 = .0, t2 = .0, t3 = .0;
    double stopThresholdLM = 0.0;
    double currentEro = 0.0;
    double currentLambda = 0.0;
    double ni = 2.0;

	//Generate init lamda
	{
        Eigen::MatrixXd H = Eigen::MatrixXd::Zero(5, 5);
        Eigen::MatrixXd g = Eigen::MatrixXd::Zero(5, 1);
        for(int i = 0; i < N; i++)
        {
            Eigen::Vector3d trans = geometry::RotationAroundY(omga) * geometry::RotationAroundZ(kappa) * Eigen::Vector3d(q1[i].x(), q1[i].y(), q1[i].z()) + Eigen::Vector3d(t1, t2, t3);
			Eigen::Vector3d residual = trans - q2[i];
            currentEro += residual.transpose() * residual;

			double x = trans.x();
			double y = trans.y();
			double z = trans.z();

			Eigen::Matrix<double, 3, 5> J_aux;
			J_aux << (t3 - z), -std::cos(omga) * (y - t2), 1.0, 0.0, 0.0,
				0.0, (std::cos(omga) * (x - t1) + std::sin(omga) * (z - t3)), 0.0, 1.0, 0.0,
				(x - t1), -std::sin(omga) * (y - t2), 0.0, 0.0, 1.0;

			H += J_aux.transpose() * J_aux;
			g -= J_aux.transpose() * residual;
		}

        stopThresholdLM = 1e-6 * currentEro;
        double maxDiagonal = 0;
        for (int i = 0; i < 5; ++i)
        {
            maxDiagonal = std::max(std::fabs(H(i, i)), maxDiagonal);
        }
        double tau = 1e-5;
        currentLambda = tau * maxDiagonal;
    }

	auto robustWeightCauchy = [=](double norm_res) -> double {
		return std::sqrt(1.0 / (1.0 + norm_res * norm_res));
	};

	int iter = 0;
	bool stop = false;
	while (!stop && iter < 30)
	{
		bool oneStepSuccess = false;
        int false_cnt = 0;

		while (!oneStepSuccess)
		{
			Eigen::MatrixXd H = Eigen::MatrixXd::Zero(5, 5);
			Eigen::MatrixXd g = Eigen::MatrixXd::Zero(5, 1);

			for (int i = 0; i < N; i++)
			{
				Eigen::Vector3d trans = geometry::RotationAroundY(omga) * geometry::RotationAroundZ(kappa) * Eigen::Vector3d(q1[i].x(), q1[i].y(), q1[i].z()) + Eigen::Vector3d(t1, t2, t3);

				double x = trans.x();
				double y = trans.y();
				double z = trans.z();

				double err_i_norm = (trans - q2[i]).norm();
				double weight = robustWeightCauchy(err_i_norm);

				Eigen::Matrix<double, 3, 5> J_aux;
				J_aux << (t3 - z), -std::cos(omga) * (y - t2), 1.0, 0.0, 0.0,
					0.0, (std::cos(omga) * (x - t1) + std::sin(omga) * (z - t3)), 0.0, 1.0, 0.0,
					(x - t1), -std::sin(omga) * (y - t2), 0.0, 0.0, 1.0;

				H += J_aux.transpose() * J_aux * weight;
				g -= J_aux.transpose() * (trans - q2[i]) * weight;
			}

			for (int i = 0; i < 5; i++)
            {
                H(i, i) += currentLambda;
            }

			Eigen::Matrix<double, 5, 1> delta = H.colPivHouseholderQr().solve(g);
            if ((delta.head(2).squaredNorm() < EPS && delta.tail(3).squaredNorm() < EPS) || false_cnt > 10)
            {
                stop = true;
                break;
            }

			omga += delta[0];
			kappa += delta[1];
			t1 += delta[2];
			t2 += delta[3];
			t3 += delta[4];

			double scale = 0;
            scale = delta.transpose() * (currentLambda * delta + g);
            scale += 1e-3;

			double temp_err = 0.0;
            for (int i = 0; i < N; i++)
            {
               	Eigen::Vector3d trans = geometry::RotationAroundY(omga) * geometry::RotationAroundZ(kappa) * Eigen::Vector3d(q1[i].x(), q1[i].y(), q1[i].z()) + Eigen::Vector3d(t1, t2, t3);
              	Eigen::Vector3d residual = trans - q2[i];
                temp_err += residual.transpose() * residual;
            }

			double rho = (currentEro - temp_err) / scale;
            if (rho > 0 && std::isfinite(temp_err)) // last step was good
            {
                double alpha = 1.0 - pow((2 * rho - 1), 3);
                alpha = std::min(alpha, 2. / 3.);
                double scaleFactor = (std::max)(1. / 3., alpha);
                currentLambda *= scaleFactor;
                ni = 2;
                currentEro = temp_err;
                oneStepSuccess = true;
            }
            else
            {
                currentLambda *= ni;
                ni *= 2;
                oneStepSuccess = false;
            }

			if(!oneStepSuccess)
            {
				omga -= delta[0];
				kappa -= delta[1];
				t1 -= delta[2];
				t2 -= delta[3];
				t3 -= delta[4];
				false_cnt++;
            }
            else
            {
                false_cnt = 0;
            }

		}
		iter++;
		if (sqrt(currentEro) <= stopThresholdLM)
			stop = true;
	}

	R = geometry::RotationAroundY(omga) * geometry::RotationAroundZ(kappa);
	T = Eigen::Vector3d(t1, t2, t3);
	error = std::sqrt(currentEro / N);
	return true;
}

bool utils::Translate3d_3d0(const std::vector<accordance_pair> &match_points, Eigen::Matrix3d &R, Eigen::Vector3d &T)
{
	if (match_points.empty())
	{
		LOG("match_points is empty, translate failed!") << std::endl;
		return false;
	}

	int N = match_points.size();
	std::vector<Eigen::Vector3d> q1(N), q2(N);
	for (int i = 0; i < N; i++)
	{
		q1[i] = match_points[i].source;
		q2[i] = match_points[i].target;
	}

	auto robustWeightCauchy = [=](double norm_res) -> double {
		return std::sqrt(1.0 / (1.0 + norm_res * norm_res));
	};

	double omga = .0, kappa = .0, t1 = .0, t2 = .0, t3 = .0;
	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * N, 5);
	Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3 * N, 1);
	Eigen::MatrixXd W = Eigen::MatrixXd::Identity(3 * N, 3 * N);

	int iter = 0;
	while (iter < 20)
	{
		for (int i = 0; i < N; i++)
		{
			Eigen::Vector3d trans = geometry::RotationAroundY(omga) * geometry::RotationAroundZ(kappa) * Eigen::Vector3d(q1[i].x(), q1[i].y(), q1[i].z()) + Eigen::Vector3d(t1, t2, t3);

			double x = trans.x();
			double y = trans.y();
			double z = trans.z();

			double err_i_norm = (trans - q2[i]).norm();
			double weight = robustWeightCauchy(err_i_norm);
			W(i * 3, i * 3) = W(i * 3 + 1, i * 3 + 1) = W(i * 3 + 2, i * 3 + 2) = weight;

			A(i * 3, 0) = (t3 - z);
			A(i * 3, 1) = -std::cos(omga) * (y - t2);
			A(i * 3, 2) = 1.0;
			A(i * 3, 3) = 0.0;
			A(i * 3, 4) = 0.0;

			A(i * 3 + 1, 0) = 0.0;
			A(i * 3 + 1, 1) = (std::cos(omga) * (x - t1) + std::sin(omga) * (z - t3));
			A(i * 3 + 1, 2) = 0.0;
			A(i * 3 + 1, 3) = 1.0;
			A(i * 3 + 1, 4) = 0.0;

			A(i * 3 + 2, 0) = (x - t1);
			A(i * 3 + 2, 1) = -std::sin(omga) * (y - t2);
			A(i * 3 + 2, 2) = 0.0;
			A(i * 3 + 2, 3) = 0.0;
			A(i * 3 + 2, 4) = 1.0;

			B(i * 3, 0) = (q2[i].x() - x);
			B(i * 3 + 1, 0) = (q2[i].y() - y);
			B(i * 3 + 2, 0) = (q2[i].z() - z);

			q1[i] = trans;
		}
		Eigen::MatrixXd delta = (A.transpose() * W * A).ldlt().solve(A.transpose() * W *  B);
		if (delta.col(0).norm() < EPS)
		{
			break;
		}
		omga += delta(0, 0);
		kappa += delta(1, 0);
		t1 += delta(2, 0);
		t2 += delta(3, 0);
		t3 += delta(4, 0);
		iter++;
	}

	R = geometry::RotationAroundY(omga) * geometry::RotationAroundZ(kappa);
	T = Eigen::Vector3d(t1, t2, t3);
	return true;
}


double utils::ComputeError(const std::vector<accordance_pair> &match_points, const Eigen::Matrix3d &R, const Eigen::Vector3d &T)
{
	double rmse = 0.0;
	int num = 0;

	for (accordance_pair match_point : match_points)
	{
		Eigen::Vector3d err = match_point.target - (R * match_point.source + T);
		double err_norm = err.norm();
		rmse += err_norm * err_norm;
		num++;
	}
	if (num != 0)
	{
		rmse = std::sqrt(rmse / num);
	}
	return rmse;
}

std::vector<hdmap_lane> utils::LoadByMiddleLayer(const GeoPoint& gps_point, const std::vector<hdmap_lane>& hdmap_lanes, double interval, double range)
{
	if(hdmap_lanes.empty())
	{
		return std::vector<hdmap_lane>();
	}
	std::vector<hdmap_lane> find_lanes;
	for(hdmap_lane lane : hdmap_lanes)
	{
		CoordinateLocalize(gps_point, lane);
		find_lanes.push_back(std::move(lane));
	}
	find_lanes = MergeHDlinks(find_lanes);
	return find_lanes;
}

void utils::CoordinateLocalize(const GeoPoint& gps_point, hdmap_lane& hdmap_lane)
{
	double heading = gps_point.bearing;
	Eigen::Vector3d n(std::cos(heading), std::sin(heading), 0.0);
	n = n.normalized();

	Eigen::Vector3d u = n.cross(Eigen::Vector3d(0, 0, 1));
	u = u.normalized();

	Eigen::Vector3d v = n.cross(u);
	v = v.normalized();

	Eigen::Matrix3d wr;
	wr << n(0), u(0), v(0), // C2W
		n(1), u(1), v(1),
		n(2), u(2), v(2);

	Eigen::Vector3d wt;
	wt << gps_point.x,
		gps_point.y,
		gps_point.z;

	for (Eigen::Vector3d &vertex : hdmap_lane.geometry)
	{
		vertex = WorldToCamera(vertex, wr, wt);
	}
}

bool utils::CalVanishPt(const std::vector<ARSLane> &arsLanes, Eigen::Vector2d &vanish_pt)
 {
     if (arsLanes.empty())
     {
         return false;
     }

     int n = arsLanes.size();

     double delta = 0.0;
     Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n, 2);
     Eigen::MatrixXd W = Eigen::MatrixXd::Identity(n, n);
	 Eigen::VectorXd B = Eigen::VectorXd::Zero(n);
     Eigen::Vector2d X(0, 0);

     auto robustWeightCauchy = [=](double norm_res) -> double {
         return 1.0 / (1.0 + norm_res * norm_res);
     };

	 int it_num = 0;

     do
     {
         for (int i = 0; i < n; i++)
         {
             A(i, 0) = arsLanes[i].line_arg2D.x();
             A(i, 1) = arsLanes[i].line_arg2D.y();
             B(i) = -arsLanes[i].line_arg2D.z();
         }
		 Eigen::Vector2d vx = (A.transpose() * W * A).ldlt().solve(A.transpose() * W * B);
		 Eigen::VectorXd err_X = A * vx - B;
		 for (int i = 0; i < n; i++)
		 {
			 W(i, i) = robustWeightCauchy(err_X[i]);
		 }

		 delta = (vx - X).norm();
		 X = vx;
		 it_num ++;
		 break;

	 } while (delta > EPS && it_num < 5);

	 vanish_pt = Eigen::Vector2d(X[0], X[1]);
	 return true;
 }

 void utils::DumpSplines3D(const std::string &filename, const std::vector<ARSLane> &lane3Ds, const std::vector<hdmap_lane> &hdlanes)
{
	std::ofstream ofs(filename);
	ofs << lane3Ds.size() << std::endl;
	for (std::size_t i = 0; i < lane3Ds.size(); i++)
	{
		ARSLane lane = lane3Ds[i];
		ofs << lane.mark_type << ",";
		for (std::size_t j = 0; j < lane.geometry3D.size() - 1; j++)
		{
			Eigen::Vector3d v = lane.geometry3D[j];
			ofs << v.x() << " " << v.y() << " " << v.z() << ",";
		}
		Eigen::Vector3d v = lane.geometry3D[lane.geometry3D.size() - 1];
		ofs << v.x() << " " << v.y() << " " << v.z() << std::endl;
	}
	ofs << hdlanes.size() << std::endl;
	for (std::size_t i = 0; i < hdlanes.size(); i++)
	{
		hdmap_lane lane = hdlanes[i];
		ofs << lane.mark_type << ",";
		for (std::size_t j = 0; j < lane.geometry.size() - 1; j++)
		{
			Eigen::Vector3d v = lane.geometry[j];
			ofs << v.x() << " " << v.y() << " " << v.z() << ",";
		}
		Eigen::Vector3d v = lane.geometry[lane.geometry.size() - 1];
		ofs << v.x() << " " << v.y() << " " << v.z() << std::endl;
	}
	ofs.close();
}